#include "eigen/eigen_equal.h"

namespace  ldl_eigen
{
bool EigenEqual::equal(const Eigen::MatrixXf &a, const Eigen::MatrixXf &b, float precision)
{
    return a.isApprox(b, precision);
}

bool EigenEqual::equal(const float a, const float b, float precision)
{
    // 处理无穷大和NaN的特殊情况
    if (std::isinf(a) || std::isinf(b)) {
        return a == b; // 只有同符号的无穷大才相等
    }
    if (std::isnan(a) || std::isnan(b)) {
        return false; // NaN 永远不等于任何值（包括自身）
    }

    // 计算绝对误差和相对误差
    float diff = std::fabs(a - b);
    float abs_a = std::fabs(a);
    float abs_b = std::fabs(b);
    float max_abs = (abs_a > abs_b) ? abs_a : abs_b;

    // 绝对误差或相对误差小于精度阈值时认为相等
    return (diff <= precision) || 
            (diff <= max_abs * precision);
}
}